#ifndef _LIDAR_IRIS_H_
#define _LIDAR_IRIS_H_

#include <vector>
#include <flann/flann.hpp>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <vector>
class LidarIris
{
public:
    struct FeatureDesc
    {
        cv::Mat1b img;
        cv::Mat1b T;
        cv::Mat1b M;
    };

    LidarIris(int nscale, int minWaveLength, float mult, float sigmaOnf, int matchNum) : _nscale(nscale),
                                                                                         _minWaveLength(minWaveLength),
                                                                                         _mult(mult),
                                                                                         _sigmaOnf(sigmaOnf),
                                                                                         _matchNum(matchNum),
                                                                                         vecList(flann::Index<flann::L2<float>>(flann::KDTreeIndexParams(4))),
                                                                                         indices(flann::Matrix<int>(new int[matchNum], 1, matchNum)),
                                                                                         dists(flann::Matrix<float>(new float[matchNum], 1, matchNum))
    {
    }
    LidarIris(const LidarIris &) = delete;
    LidarIris &operator=(const LidarIris &) = delete;

    static cv::Mat1b GetIris(const pcl::PointCloud<pcl::PointXYZI> &cloud);
     
    static cv::Mat1b GetIrisWithI(const pcl::PointCloud<pcl::PointXYZI> &cloud);
    static cv::Mat1b GetRingKeyFromIris(cv::Mat1b li);
    //static cv::Mat1b GetRingKeyFromIrisWithI(cv::Mat1b li);

    //
    void UpdateFrame(const cv::Mat1b &frame, int frameIndex, float *matchDistance, int *matchIndex);
    //
    float Compare(const FeatureDesc &img1, const FeatureDesc &img2, int *bias = nullptr);
    //
    void makeAndSaveLidarIris( pcl::PointCloud<pcl::PointXYZI>::Ptr & _iris_down );
     FeatureDesc GetFeature(const cv::Mat1b &src);
     FeatureDesc GetFeature(const cv::Mat1b &src, std::vector<float> &vec);
    std::vector<cv::Mat2f> LogGaborFilter(const cv::Mat1f &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf);
    void GetHammingDistance(const cv::Mat1b &T1, const cv::Mat1b &M1, const cv::Mat1b &T2, const cv::Mat1b &M2, int scale, float &dis, int &bias);
    //
    static inline cv::Mat circRowShift(const cv::Mat &src, int shift_m_rows);
    static inline cv::Mat circColShift(const cv::Mat &src, int shift_n_cols);
    static cv::Mat circShift(const cv::Mat &src, int shift_m_rows, int shift_n_cols);
    std::pair<float, int> distanceBtnLidarIris( cv::Mat1b &_sc1,cv::Mat1b &_sc2 );
    const int    SC_PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
    const int    SC_PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
    double SC_PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
    const double SC_PC_UNIT_SECTORANGLE = 360.0 / double(SC_PC_NUM_SECTOR);
    const double SC_PC_UNIT_RINGGAP = SC_PC_MAX_RADIUS / double(SC_PC_NUM_RING);

    // tree
    const int    SC_NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. 
    const int    SC_NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper)

    // loop thres
    const double SC_SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
    // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)

    double SC_DIST_THRES = 0.2; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
    // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15

    // config 
    const int    SC_TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N.
    int          SC_tree_making_period_conter = 0;
const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
private:
    void LoGFeatureEncode(const cv::Mat1b &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf, cv::Mat1b &T, cv::Mat1b &M);

    int _nscale;
    int _minWaveLength;
    float _mult;
    float _sigmaOnf;
    int _matchNum;
    
    flann::Index<flann::L2<float>> vecList;
    std::vector<FeatureDesc> featureList;
    std::vector<int> frameIndexList;
    flann::Matrix<int> indices;
    flann::Matrix<float> dists;
};

#endif
